clear all;
close all;

alphas=0.1;betas=0.1;m1=5/3;m2=5/9;epsilon=0.1;J=2;ksi=0.5;alphar=2.5;betar=2.5;m3=5/3;m4=7/9;

Qk=[0.5;0];

T=0.001;
for k=1:1:10000
  
    time(k)=k*T;

    thd(k)=0;         
    th(k)=Qk(1);
    dth(k)=Qk(2);

    dt(k)=0.1+0.2*sin(2*k*T);
    D=dt(k);

    e(k)=thd(k)-th(k);
    de(k)=-dth(k);

    

    k1 = m1^(sign(abs(e(k))-1));
    k2 = m2^(sign(abs(e(k))-1));
    l1 = (2-k1)*epsilon^(k1-1);
    g1 = (k1-1)*epsilon^(k1-2); 
    l2 = (2-k2)*epsilon^(k2-1); 
    g2 = (k2-1)*epsilon^(k2-2);


    if abs(e(k))>epsilon
        ce1=sign(e(k))*abs(e(k))^k1;
        ce2=sign(e(k))*abs(e(k))^k2;
        dce1=k1*sign(e(k))*abs(e(k))^(k1-1)*de(k);
        dce2=k2*sign(e(k))*abs(e(k))^(k2-1)*de(k);
    else 
        ce1=l1*e(k)+g1*e(k)^2*sign(e(k));
        ce2=l2*e(k)+g2*e(k)^2*sign(e(k));
        dce1=l1*e(k)+g1*2*e(k)*sign(e(k))*de(k);
        dce2=l2*e(k)+g2*2*e(k)*sign(e(k))*de(k);
    end

    s(k)=de(k)+ce1+ce2;
    k3 = m3^(sign(abs(s(k))-1));
    k4 = m4^(sign(1-abs(s(k))));
    
    U(k)=J*(dce1+dce2+alphar*sign(s(k))*abs(s(k))^(k3)+betar*sign(s(k))*abs(s(k))^(k4))+ksi*sign(s(k));
    vt=U(k);

    tSpan = [0 T];
	[tt,Qq] = ode45(@(t,x) plant(t,x,vt,D),tSpan,Qk);
    Qk = Qq(length(Qq),:);
     
end

figure(1);
plot(time,th,'k',time,thd,'r--');
xlabel('time(s)');ylabel('th');
legend('th');
grid on;

figure(2);
plot(time,U,'k');
xlabel('time(s)');ylabel('U');
legend('U');
 

